#include "ODRoadGeom.h"
#include <algorithm>

#undef near
#undef far

namespace ODRoadGeom
{
    using namespace std;

    static const double skipdis = 50.0;

    double ODCfgParameters::step = 200.0;
    double ODCfgParameters::roadMarkStep = 100.0;
    double ODCfgParameters::uvlength = 1000.0;

    static glm::dvec3 GetRandomTangent(const glm::dvec3& Normal)
    {
        double X = Normal.x;
        double Y = Normal.y;
        double Z = Normal.z;

        if (std::fabs(X) >= std::fabs(Y) && std::fabs(X) >= std::fabs(Z))
            return glm::dvec3((X < 0.0f ? -1.0f : 1.0f) * (-Y - Z), std::fabs(X), std::fabs(X));
        if (std::fabs(Y) >= std::fabs(X) && std::fabs(Y) >= std::fabs(Z))
            return glm::dvec3(std::fabs(Y), (Y < 0.0f ? -1.0f : 1.0f) * (-X - Y), std::fabs(Y));
        return glm::dvec3(std::fabs(Z), std::fabs(Z), (Z < 0.0f ? -1.0f : 1.0f) * (-X - Y));
    }

    //static int32_t GetOppSide(int32_t side)
    //{
    //    return side == 0 ? 0 : side * -1;
    //}

    //static int32_t GetOppPos(int32_t pos)
    //{
    //    return pos * -1;
    //}

    /* OPENDRIVE System*/
    ODSystem  *ODSystem::_instance = NULL;

    ODSystem  *ODSystem::Instance()
    {
        if (_instance == NULL)
        {
            _instance = new ODSystem();
        }
        return _instance;
    }

    void ODSystem::Destroy()
    {
        if (_instance != NULL)
        {
            delete _instance;
        }

        _instance = NULL;
    }

    ODSystem::ODSystem() {}

    ODRoadPtr     ODSystem::GetRoad(const string& id) const
    {
        ODRoadPtr ret = nullptr;
        map<string, ODRoadPtr >::const_iterator iter = roadMap.find(id);
        if (iter != roadMap.end())
        {
            ret = iter->second;
        }
        return ret;
    }

    ODJunctionPtr ODSystem::GetJunction(const string& id) const
    {
        ODJunctionPtr ret = nullptr;

        map<string, ODJunctionPtr>::const_iterator iter = junctionMap.find(id);
        if (iter != junctionMap.end())
        {
            ret = iter->second;
        }
        return ret;
    }


    ODSignalPtr ODSystem::GetSignal(const string& id) const
    {
        ODSignalPtr ret = nullptr;

        std::map<string, ODSignalPtr>::const_iterator iter = signalMap.find(id);
        if (iter != signalMap.end())
        {
            ret = iter->second;
        }
        return ret;
    }


    void ODSystem::GenerateVertices()
    {
        for (const auto& RoadIt : roadMap)
        {
            ODRoadPtr road = RoadIt.second;
            road->GenerateVertices();
            road->GenerateRoadMarks();
        }

        for (auto& jIt : junctionMap)
        {
            ODJunctionPtr junction = jIt.second;
            junction->GenerateVertices(*this);
        }
    }

    void ODSystem::AddSignal(double s, double t, const string& id, const string& name, const string& bDynamic, const string& orientation, double zOffset, const string& country, const string& type, const string& subtype, double value, const string& unit, double height, double width, const string& text, double hOffset, double pitch, double roll)
    {
        ODSignalPtr p(new ODSignal(s, t, id, name, bDynamic, orientation, zOffset, country, type, subtype, value, unit, height, width, text, hOffset, pitch, roll));
        signalMap[id] = p;
    }


    /* road element */
    ODElement::ODElement(const string& s_id, const string& s_name)
        :id(s_id), name(s_name)
    {
    }

    string ODElement::GetId() const
    {
        return id;
    }

    string ODElement::GetName() const
    {
        return name;
    }
    /* road element */


    /*  OPENDRIVE Controller */
    ODController::ODController(const string& s_id, const string& s_name, int32_t in_seq)
        : ODElement(s_id, s_name), sequence(in_seq)
    {}


    /* OPENDRIVE road */
    ODRoad::ODRoad(const string& s_id, const string& s_name)
        : ODElement(s_id, s_name)
    {}

    // add plan view
    void ODRoad::AddPlanLine(double s, double in_xs, double in_ys, double in_hdg, double in_length)
    {
        ODPlanCurvePtr p(new PlanLine(s, in_xs, in_ys, in_hdg, in_length));
        geometries.push_back(p);
    }
    void ODRoad::AddPlanArc(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cr)
    {
        ODPlanCurvePtr p(new PlanArc(s, in_xs, in_ys, in_hdg, in_length, 1 / in_cr));
        geometries.push_back(p);
    }
    void ODRoad::AddPlanSpiral(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_cs, double in_ce)
    {
        ODPlanCurvePtr p(new PlanSpiral(s, in_xs, in_ys, in_hdg, in_length, in_cs, in_ce));
        geometries.push_back(p);
    }
    void ODRoad::AddPlanPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_a, double in_b, double in_c, double in_d)
    {
        ODPlanCurvePtr p(new PlanPoly3(s, in_xs, in_ys, in_hdg, in_length, in_a, in_b, in_c, in_d));
        geometries.push_back(p);
    }
    void ODRoad::AddParamPoly3(double s, double in_xs, double in_ys, double in_hdg, double in_length, double in_aU, double in_bU, double in_cU, double in_dU, double in_aV, double in_bV, double in_cV, double in_dV, double in_pRange)
    {
        ODPlanCurvePtr p(new PlanParamPoly3(s, in_xs, in_ys, in_hdg, in_length, in_aU, in_bU, in_cU, in_dU, in_aV, in_bV, in_cV, in_dV, in_pRange));
        geometries.push_back(p);
    }

    // add elevation
    void ODRoad::AddElevation(double in_s, double in_a, double in_b, double in_c, double in_d)
    {
        ODPolynomCurvePtr p(new ODPolynomCurve(in_s, in_a, in_b, in_c, in_d));
        elevations.push_back(p);
    }

    // add lane section
    void ODRoad::AddLaneSection(ODLaneSectionPtr ls)
    {
        laneSections.push_back(ls);
    }

    // add signal
    void ODRoad::AddSignalRef(double s, double t, const string& in_id, const string& orientation)
    {
        ODSignalRefPtr p(new ODSignalReference(s, t, in_id, orientation));
        signalRefs.push_back(p);
    }

    // evaluate
    void ODRoad::EvaluatePoint(double s, glm::dvec3& pos)
    {
        EvaluatePoint2D(s, pos);
        pos.z = EvaluateElevation(s);
    }

    void ODRoad::EvaluatePoint2D(double s, glm::dvec3& pos)
    {
        vector<ODPlanCurvePtr>::reverse_iterator iter = geometries.rbegin();
        for (; iter != geometries.rend(); ++iter)
        {
            ODPlanCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            glm::dvec3 out = curve->GetPoint(s);
            pos.x = out.x;
            pos.y = out.y;
            pos.z = out.z;
            break;
        }
    }

    double ODRoad::EvaluateElevation(double s)
    {
        //return 0;
        std::vector<ODPolynomCurvePtr >::reverse_iterator iter = elevations.rbegin();
        for (; iter != elevations.rend(); ++iter)
        {
            ODPolynomCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            return curve->GetValue(s);
        }

        return 0;
    }

    double ODRoad::EvaluateSlope(double s)
    {
        std::vector<ODPolynomCurvePtr >::reverse_iterator iter = elevations.rbegin();
        for (; iter != elevations.rend(); ++iter)
        {
            ODPolynomCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            return curve->GetSlope(s);
        }

        return 0;
    }

    void ODRoad::EvaluatePoint(double s, double t, glm::dvec3& pos)
    {
        glm::dvec3 xyPoint(0, 0, 0);
        EvaluatePoint2D(s, xyPoint);
        double zPoint = EvaluateElevation(s);


        double alpha = 0;
        double beta = EvaluateSlope(s);
        double gamma = xyPoint.z;

        double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma));
        double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma));
        double Tz = (sin(alpha) * cos(beta));

        pos = glm::dvec3(xyPoint.x + Tx * t, -(xyPoint.y + Ty * t), zPoint + Tz * t);
    }

    double ODRoad::EvaluateWidth(double s, int32_t side)
    {
        ODLaneSectionPtr laneSec = GetLaneSection(s);
        if (laneSec != nullptr)
        {
            return laneSec->GetSideWidth(s, side);
        }
        return 0;
    }

    double ODRoad::EvaluateWidth(double s, int32_t side, const string& laneType)
    {
        ODLaneSectionPtr laneSec = GetLaneSection(s);
        if (laneSec != nullptr)
        {
            return laneSec->GetSideWidth(s, side, laneType);
        }
        return 0;
    }

    // get orientation
    double ODRoad::GetOrientation(double s)
    {
        vector<ODPlanCurvePtr>::reverse_iterator iter = geometries.rbegin();
        for (; iter != geometries.rend(); ++iter)
        {
            ODPlanCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            return curve->GetOrientation(s);
        }

        return 0;
    }

    ODPlanCurvePtr ODRoad::GetGeometry(double s)
    {
        vector<ODPlanCurvePtr>::reverse_iterator iter = geometries.rbegin();
        for (; iter != geometries.rend(); ++iter)
        {
            ODPlanCurvePtr curve = *iter;
            if (s > curve->start)
                return curve;
        }
        return nullptr;
    }

    ODPolynomCurvePtr ODRoad::GetElevation(double s)
    {
        vector<ODPolynomCurvePtr>::reverse_iterator iter = elevations.rbegin();
        for (; iter != elevations.rend(); ++iter)
        {
            ODPolynomCurvePtr curve = *iter;
            if (s > curve->start)
                return curve;
        }

        return nullptr;
    }

    ODLaneSectionPtr ODRoad::GetLaneSection(double s)
    {
        std::vector<ODLaneSectionPtr>::reverse_iterator iter = laneSections.rbegin();
        for (; iter != laneSections.rend(); ++iter)
        {
            ODLaneSectionPtr ls = *iter;
            if (s > ls->start)
                return ls;
        }
        return nullptr;
    }


    void ODRoad::GenerateVertices()
    {
        centerRoad.clear();

        const double h = ODCfgParameters::step;

        double l = 0;
        glm::dvec3 point(0, 0, 0);
        while (l <= length)
        {
            EvaluatePoint(l, point);
            point.y *= -1;
            centerRoad.push_back(point);
            l += h;
            if (l > length)
            {
                EvaluatePoint(length, point);
                point.y *= -1;
                centerRoad.push_back(point);
                break;
            }
        }



        std::vector<ODLaneSectionPtr>::iterator lsIt;
        ODLaneSectionPtr ls = nullptr;
        for (lsIt = laneSections.begin(); lsIt != laneSections.end(); ++lsIt)
        {
            double lsStart;
            double lsEnd;

            std::vector<ODLaneSectionPtr>::iterator nextLsIt = lsIt;
            ++nextLsIt;
            ls = *lsIt;

            lsStart = ls->start;

            if (lsStart > length)
            {
                break;
            }
            else if (nextLsIt == laneSections.end())
            {
                lsEnd = length;
            }
            else
            {
                lsEnd = (*nextLsIt)->start;
                if (lsEnd > length)
                    lsEnd = length;
            }

            int leftcount = ls->LeftlaneMap.size();
            int rightcount = ls->RightlaneMap.size();

            for (int i = leftcount; i >= -rightcount; --i)
            {
                bool lastRound = false;

                for (double s = lsStart; s < (lsEnd + h); s += h)
                {
                    if (s > lsEnd)
                    {
                        s = lsEnd;
                        lastRound = true;
                    }

                    if (i < 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->RightlaneMap.find(i);
                        if (iter != ls->RightlaneMap.end())
                        {
                            iter->second->GenerateLanePoint(s, i, ls, *this, lastRound);
                        }
                    }
                    else if (i > 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->LeftlaneMap.find(i);
                        if (iter != ls->LeftlaneMap.end())
                        {
                            iter->second->GenerateLanePoint(s, i, ls, *this, lastRound);
                        }
                    }
                    else if (i == 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->CenterlaneMap.find(i);
                        if (iter != ls->CenterlaneMap.end())
                        {
                            iter->second->GenerateLanePoint(s, i, ls, *this, lastRound);
                        }
                    }
                    if (lastRound)
                    {
                        s = lsEnd + h;
                    }
                }
            }
        }
    }

    void ODRoad::GenerateRoadMarks()
    {
        const double h = ODCfgParameters::roadMarkStep;

        std::vector<ODLaneSectionPtr>::iterator lsIt;
        ODLaneSectionPtr ls = nullptr;
        for (lsIt = laneSections.begin(); lsIt != laneSections.end(); ++lsIt)
        {
            double lsStart;
            double lsEnd;

            std::vector<ODLaneSectionPtr>::iterator nextLsIt = lsIt;
            ++nextLsIt;
            ls = *lsIt;

            lsStart = ls->start;

            if (lsStart > length)
            {
                break;
            }
            else if (nextLsIt == laneSections.end())
            {
                lsEnd = length;
            }
            else
            {
                lsEnd = (*nextLsIt)->start;
                if (lsEnd > length)
                    lsEnd = length;
            }

            int leftcount = ls->LeftlaneMap.size();
            int rightcount = ls->RightlaneMap.size();

            for (int32_t i = leftcount; i >= -rightcount; --i)
            {
                bool lastRound = false;

                for (double s = lsStart; s < (lsEnd + h); s += h)
                {
                    if (s > lsEnd)
                    {
                        s = lsEnd;
                        lastRound = true;
                    }

                    if (i < 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->RightlaneMap.find(i);
                        if (iter != ls->RightlaneMap.end())
                        {
                            iter->second->GenerateRoadMark(s, i, ls, *this);
                        }
                    }
                    else if (i > 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->LeftlaneMap.find(i);
                        if (iter != ls->LeftlaneMap.end())
                        {
                            iter->second->GenerateRoadMark(s, i, ls, *this);
                        }
                    }
                    else if (i == 0)
                    {
                        std::map<int, ODLanePtr>::iterator iter = ls->CenterlaneMap.find(i);
                        if (iter != ls->CenterlaneMap.end())
                        {
                            iter->second->GenerateRoadMark(s, i, ls, *this);
                        }
                    }
                    if (lastRound)
                    {
                        s = lsEnd + h;
                    }
                }
            }
        }
    }

    void ODRoad::CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats) const
    {
        for (const auto& lanesecIt : laneSections)
        {
            ODLaneSectionPtr sec = lanesecIt;

            sec->CollectTriangles(vertices, normals, tangent, binormal, uvs, indices, count, mats);
        }
    }

    void ODRoad::CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats) const
    {
        for (const auto& lanesecIt : laneSections)
        {
            ODLaneSectionPtr sec = lanesecIt;

            sec->CollectRoadMarkTriangles(vertices, normals, tangent, binormal, uvs, indices, count, mats);
        }
    }

    const vector<glm::dvec3>& ODRoad::CollectCenter() const
    {
        return centerRoad;
    }

    glm::dvec3 ODRoad::GetEdgePoint(int32_t side, int32_t pos)
    {
        ODLaneSectionPtr ls = (pos == -1) ? laneSections.back() : laneSections.front();
        glm::dvec3 ret((pos == -1) ? centerRoad.back() : centerRoad.front());
        ls->GetEdgePoint(side, pos, ret);
        return ret;
    }

    glm::dvec3 ODRoad::GetHeadPoint(int32_t pos)
    {
        return (pos == -1) ? centerRoad.back() : centerRoad.front();
    }

    void ODRoad::FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint)
    {
        for (const auto& lanesecIt : laneSections)
        {
            ODLaneSectionPtr sec = lanesecIt;

            sec->FindClosetPointWith(input, outpoint);
        }
    }

    int32_t ODRoad::GetHeadCenterPointbyRef(glm::dvec3& out_point, const ODElement& refObj, ODLink::LinkType type)
    {
        int32_t refPos = IsConnected( refObj.GetId(), type);
        if (refPos != 0)
        {
            glm::dvec3 left = GetEdgePoint(1, refPos);
            glm::dvec3 right = GetEdgePoint(-1, refPos);

            out_point = (left + right) * 0.5;
        }
        return refPos;

    }

    int32_t    ODRoad::IsConnected(const string& linked, ODLink::LinkType linktype)
    {
        if (predecessor != nullptr)
        {
            if (predecessor->linkType == linktype)
            {
                if (linked == predecessor->link)
                {
                    return 1;
                }
            }
        }
        if (successor != nullptr)
        {
            if (successor->linkType == linktype)
            {
                if (linked == successor->link)
                {
                    return -1;
                }
            }
        }

        return 0;
    }


    int32_t ODRoad::FindLaneLinkedIdWith(const string& linked, int32_t inId)
    {
        if (inId == 0)
            return 0;

        if (predecessor != nullptr)
        {
            if (predecessor->link == linked)
            {
                {
                    return laneSections.front()->FindLaneLinkedIdWith(inId);
                }
            }
        }
        if (successor != nullptr)
        {
            if (successor->link == linked)
            {
                {
                    return laneSections.back()->FindLaneLinkedIdWith(inId);
                }
            }
        }
        return 0;
    }

    void ODRoad::CollectBoundary(int32_t side, vector<glm::dvec3>& array, std::uint32_t& count, vector<int32_t>* indices, vector<glm::dvec3>* normals, vector<glm::dvec2>* uvs)
    {
        for (const auto& lsIt : laneSections)
        {
            (*lsIt).CollectBoundary(side, array, count, indices, normals, uvs);
        }
    }

    void ODRoad::GetLinkEdgePointByRef(int32_t side, glm::dvec3& out_point, const ODJunction& junction)
    {
        if (predecessor != nullptr)
        {
            {
                if ( predecessor->link == junction.GetId() && predecessor->linkType == ODLink::ELT_JUNCTION )
                {
                    out_point = GetEdgePoint(side, 1);
                }
            }
        }
        if (successor != nullptr)
        {
            {
                if (successor->link == junction.GetId() && successor->linkType == ODLink::ELT_JUNCTION )
                {
                    out_point = GetEdgePoint(side, -1);
                }
            }
        }
    }

    /* ODLaneSection*/
    ODLaneSection::ODLaneSection(double s)
        : start(s)
    {}

    void ODLaneSection::AddLane(ODLanePtr lane)
    {
        int id = lane->id;
        if (id > 0)
            LeftlaneMap[id] = lane;
        else if (id == 0)
            CenterlaneMap[id] = lane;
        else if (id < 0)
            RightlaneMap[id] = lane;
    }

    double ODLaneSection::GetLaneWidth(double s, int id)
    {
        s -= start;
        if (id > 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = LeftlaneMap.find(id);
            if (laneIt != LeftlaneMap.end())
            {
                return (laneIt->second)->GetWidth(s);
            }
        }
        else if (id < 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = RightlaneMap.find(id);
            if (laneIt != RightlaneMap.end())
            {
                return (laneIt->second)->GetWidth(s) * -1;
            }
        }
        return 0.0;
    }

    double ODLaneSection::GetSideWidth(double s, int32_t side)
    {
        s -= start;
        double ret = 0;
        if (side > 0)
        {
            for (auto& laneIt : LeftlaneMap)
            {
                ret += (laneIt.second)->GetWidth(s);
            }
        }
        else if (side < 0)
        {
            for (auto& laneIt : RightlaneMap)
            {
                ret += (laneIt.second)->GetWidth(s);
            }
        }

        return ret;
    }

    double ODLaneSection::GetSideWidth(double s, int32_t side, const string& laneType)
    {
        s -= start;
        double ret = 0;
        if (side > 0)
        {
            for (auto& laneIt : LeftlaneMap)
            {
                if ((laneIt.second)->laneType == laneType)
                {
                    ret += (laneIt.second)->GetWidth(s);
                }
            }
        }
        else if (side < 0)
        {
            for (auto& laneIt : RightlaneMap)
            {
                if ((laneIt.second)->laneType == laneType)
                {
                    ret += (laneIt.second)->GetWidth(s);
                }
            }
        }

        return ret;
    }

    double ODLaneSection::GetDistanceToLane(double s, int id)
    {
        s -= start;
        int it = (id > 0) ? 1 : -1;
        double d = 0;

        if (id > 0)
        {
            for (int i = 0; i != id; i += it)
            {
                std::map<int, ODLanePtr>::iterator laneIt = LeftlaneMap.find(i);
                if (laneIt != LeftlaneMap.end())
                {
                    d += laneIt->second->GetWidth(s);
                }
            }
        }

        else if (id < 0)
        {
            for (int i = 0; i != id; i += it)
            {
                std::map<int, ODLanePtr>::iterator laneIt = RightlaneMap.find(i);
                if (laneIt != RightlaneMap.end())
                {
                    d += laneIt->second->GetWidth(s);
                }
            }
        }

        else
        {
            d = 0;
        }
        return it * d;
    }


    double ODLaneSection::GetHightInnerOffset(double s, int id)
    {
        if (id == 0)  return 0;

        s -= start;
        if (id > 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = LeftlaneMap.find(id);
            if (laneIt != LeftlaneMap.end())
            {
                return (laneIt->second)->GetHeight(s, 0);
            }
        }
        else if (id < 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = RightlaneMap.find(id);
            if (laneIt != RightlaneMap.end())
            {
                return (laneIt->second)->GetHeight(s, 0);
            }
        }
        return 0;
    }

    double ODLaneSection::GetHightOuterOffset(double s, int id)
    {
        if (id == 0)  return 0;

        s -= start;
        if (id > 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = LeftlaneMap.find(id);
            if (laneIt != LeftlaneMap.end())
            {
                return (laneIt->second)->GetHeight(s, 1);
            }
        }
        else if (id < 0)
        {
            std::map<int, ODLanePtr>::iterator laneIt = RightlaneMap.find(id);
            if (laneIt != RightlaneMap.end())
            {
                return (laneIt->second)->GetHeight(s, 1);
            }
        }
        return 0;
    }

    void ODLaneSection::CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats)
    {
        for (const auto& laneIt : LeftlaneMap)
        {
            ODLanePtr le = laneIt.second;
            le->CollectTriangles(vertices, normals, tangent, binormal, uvs, indices, count, 0, mats);
        }
        for (const auto& laneIt : RightlaneMap)
        {
            ODLanePtr le = laneIt.second;
            le->CollectTriangles(vertices, normals, tangent, binormal, uvs, indices, count, 1, mats);
        }
    }

    void ODLaneSection::CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangent, vector<glm::dvec3>& binormal, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats)
    {
        for (const auto& laneIt : LeftlaneMap)
        {
            ODLanePtr le = laneIt.second;
            le->CollectRoadMarkTriangles(vertices, normals, tangent, binormal, uvs, indices, count, 1, mats);
        }
        for (const auto& laneIt : RightlaneMap)
        {
            ODLanePtr le = laneIt.second;
            le->CollectRoadMarkTriangles(vertices, normals, tangent, binormal, uvs, indices, count, 1, mats);
        }
        for (const auto& laneIt : CenterlaneMap)
        {
            ODLanePtr le = laneIt.second;
            le->CollectRoadMarkTriangles(vertices, normals, tangent, binormal, uvs, indices, count, 1, mats);
        }
    }

    void ODLaneSection::GetEdgePoint(int32_t side, int32_t pos, glm::dvec3& out_point)
    {        
        std::map<int, ODLanePtr>&  lanesRef = (side == 1) ? LeftlaneMap : RightlaneMap;
        if (lanesRef.size() == 0)
        {
            return;
        }

        ODLanePtr lane = nullptr;
        int maxIndex = 0;
        std::map<int, ODLanePtr>::iterator iter = lanesRef.begin();
        for (; iter != lanesRef.end(); ++iter)
        {
            int id = iter->first;
            if (abs(id) > maxIndex)
            {
                lane = iter->second;
                maxIndex = abs(id);
            }
        }

        if (lane != nullptr)
        {
            glm::dvec3 p = (pos == -1) ? lane->fars.back() : lane->fars.front();
            out_point.x = p.x;
            out_point.y = p.y;
            out_point.z = p.z;
            if (lane->lowfars.size() > 0)
            {
                glm::dvec3 low = (pos == -1) ? lane->lowfars.back() : lane->lowfars.front();
                out_point.z = low.z;
            }
        }
    }

    void ODLaneSection::CollectBoundary(int32_t side, vector<glm::dvec3>& array, std::uint32_t& count, vector<int32_t>* indices, vector<glm::dvec3>* normals, vector<glm::dvec2>* uvs) const
    {
        if (side == 0)
            return;
        std::map<int, ODLanePtr>  lanesRef = (side == 1) ? LeftlaneMap : RightlaneMap;
        if (lanesRef.size() == 0)
        {
            return;
        }

        ODLanePtr lane = nullptr;
        int maxIndex = 0;
        std::map<int, ODLanePtr>::iterator iterlane = lanesRef.begin();
        for (; iterlane != lanesRef.end(); ++iterlane)
        {
            int id = iterlane->first;
            if (abs(id) > maxIndex)
            {
                lane = iterlane->second;
                maxIndex = abs(id);
            }
        }

        if (lane != nullptr)
        {
            for (std::vector<glm::dvec3>::iterator iter = lane->fars.begin(); iter != lane->fars.end(); ++iter)
            {
                array.push_back(*iter);
                ++count;
                if (indices != nullptr)
                {
                    indices->push_back(count);
                }
            }

            if (normals != nullptr)
            {
                for (std::vector<glm::dvec3>::iterator iter = lane->farNormals.begin(); iter != lane->farNormals.end(); ++iter)
                {
                    normals->push_back(*iter);
                }
            }
            if (uvs != nullptr)
            {
                for (std::vector<glm::dvec2>::iterator iter = lane->farUVs.begin(); iter != lane->farUVs.end(); ++iter)
                {
                    uvs->push_back(*iter);
                }
            }
        }
    }

    void ODLaneSection::FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint)
    {
        for (const auto& leftlane : LeftlaneMap)
        {
            ODLanePtr l = leftlane.second;

            l->FindClosetPointWith(input, outpoint);
        }
        for (const auto& rightlane : RightlaneMap)
        {
            ODLanePtr l = rightlane.second;

            l->FindClosetPointWith(input, outpoint);
        }
    }

    int32_t ODLaneSection::GetOuterLaneId(int32_t side)
    {
        if (side > 0)
        {
            int32_t ret = -1;
            for (auto& lane : LeftlaneMap)
            {
                if (lane.first > ret)
                {
                    ret = lane.first;
                }
            }
            return ret;
        }
        else if (side < 0)
        {
            int32_t ret = 1;
            for (auto& lane : RightlaneMap)
            {
                if (lane.first < ret)
                {
                    ret = lane.first;
                }
            }
            return ret;
        }

        return 0;
    }

    int32_t ODLaneSection::FindLaneLinkedIdWith(int32_t laneid)
    {
        if (laneid > 0)
        {
            for (auto& lane : LeftlaneMap)
            {
                if (lane.first == laneid)
                {
                    return lane.second->successor;
                }
            }
        }
        else if (laneid < 0)
        {
            for (auto& lane : RightlaneMap)
            {
                if (lane.first == laneid)
                {
                    return lane.second->successor;
                }
            }
        }
        return 0;
    }

    /* ODLane */
    const int ODLane::NOLANE = 0xdeadbeef;

    ODLane::ODLane(int in_id, const string& typestring, const string& levelstring)
        : id(in_id), laneType(typestring), predecessor(0), successor(0)
    {
        if (laneType == string("none"))
        {
            laneType = string("noname");
        }

        if (levelstring == string("true"))
        {
            level = true;
        }
        else
        {
            level = false;
        }
    }

    void ODLane::AddWidth(double s, double a, double b, double c, double d)
    {
        ODPolynomCurvePtr p(new ODPolynomCurve(s, a, b, c, d));
        widths.push_back(p);
    }

    double ODLane::GetWidth(double s)
    {
        vector<ODPolynomCurvePtr>::reverse_iterator iter = widths.rbegin();
        for (; iter != widths.rend(); ++iter)
        {
            ODPolynomCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            return curve->GetValue(s);
        }
        return 0;
    }

    void ODLane::AddHeight(double s, double ier, double oer)
    {
        ODLinearCurvePtr p(new ODLinearCurve(s, ier, oer));
        heights.push_back(p);
    }

    double ODLane::GetHeight(double s, int i)
    {
        vector<ODLinearCurvePtr>::reverse_iterator iter = heights.rbegin();
        for (; iter != heights.rend(); ++iter)
        {
            ODLinearCurvePtr curve = *iter;
            if (s < curve->start)
                continue;
            return curve->GetValue(i);
        }
        return 0;
    }

    void ODLane::AddRoadMark(double in_start, string stype, string sweight, string scolor, string material, double in_width, string slaneChange, double in_height)
    {
        ODRoadMarkCurvePtr p(new ODRoadMarkCurve(in_start, stype, sweight, scolor, material, in_width, slaneChange, in_height));
        roadmarks.push_back(p);
    }

    ODPolynomCurvePtr   ODLane::GetWidthCurve(double s)
    {
        vector<ODPolynomCurvePtr>::reverse_iterator iter = widths.rbegin();
        for (; iter != widths.rend(); ++iter)
        {
            ODPolynomCurvePtr curve = *iter;
            if (s > curve->start)
                return curve;
        }
        return nullptr;
    }

    ODLinearCurvePtr      ODLane::GetHeightCurve(double s)
    {
        std::vector<ODLinearCurvePtr>::reverse_iterator iter = heights.rbegin();
        for (; iter != heights.rend(); ++iter)
        {
            ODLinearCurvePtr curve = *iter;
            if (s > curve->start)
                return curve;
        }
        return nullptr;
    }

    ODRoadMarkCurvePtr ODLane::GetRoadmarkCurve(double s)
    {
        std::vector<ODRoadMarkCurvePtr>::reverse_iterator iter = roadmarks.rbegin();
        for (; iter != roadmarks.rend(); ++iter)
        {
            ODRoadMarkCurvePtr curve = *iter;
            if (s > curve->start)
                return curve;
        }
        return nullptr;
    }

    void ODLane::GenerateLanePoint(double s, int i, ODLaneSectionPtr owner, ODRoad& road, bool lastRound)
    {
        double disIn = owner->GetDistanceToLane(s, i);
        double disOut = disIn + owner->GetLaneWidth(s, i);

        glm::dvec3 xyPoint(0, 0, 0);
        road.EvaluatePoint2D(s, xyPoint);
        double zPoint = road.EvaluateElevation(s);


        double alpha = 0;
        double beta = road.EvaluateSlope(s);
        double gamma = xyPoint.z;

        double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma));
        double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma));
        double Tz = (sin(alpha) * cos(beta));

        double nx = sin(alpha) * sin(gamma) + cos(alpha) * sin(beta) * cos(gamma);
        double ny = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
        double nz = cos(alpha) * cos(beta);

        glm::dvec3 in = glm::dvec3(xyPoint.x + Tx * disIn, -(xyPoint.y + Ty * disIn), zPoint + Tz * disIn);
        glm::dvec3 out = glm::dvec3(xyPoint.x + Tx * disOut, -(xyPoint.y + Ty * disOut), zPoint + Tz * disOut);


        double innerH = owner->GetHightInnerOffset(s, i);
        double outerH = owner->GetHightOuterOffset(s, i);

        glm::dvec3 inNormal = glm::dvec3(nx, ny, nz);
        glm::dvec3 outNormal = glm::dvec3(nx, ny, nz);

        int k = (int)(s / ODCfgParameters::uvlength);
        int d = (int)(i / fabs(i));

        glm::dvec2 inUV = glm::dvec2(0.5f + (double)d * disIn / ODCfgParameters::uvlength, (double)s / ODCfgParameters::uvlength);
        glm::dvec2 outUV = glm::dvec2(0.5f + (double)d* disOut / ODCfgParameters::uvlength, (double)s / ODCfgParameters::uvlength);

        if (innerH > 0)
        {
            if (lownears.size() == 0 && nears.size() != 0)
            {
                glm::dvec3 normal = glm::dvec3(nx, -nz, ny);
                lownears.push_back(nears[nears.size() - 1]);
                lownearnormals.push_back(normal);
                glm::dvec2 lowUV = glm::dvec2(0, s / ODCfgParameters::uvlength - (double)k);
                lownearuvs.push_back(lowUV);

                highnears.push_back(nears[nears.size() - 1]);
                highnearnormals.push_back(normal);
                glm::dvec2 highUV = glm::dvec2(1, s / ODCfgParameters::uvlength - (double)k);
                highnearuvs.push_back(highUV);

            }
            glm::dvec3 normal = glm::dvec3(nx, -nz, ny);
            lownears.push_back(in);
            lownearnormals.push_back(normal);
            glm::dvec2 lowUV = glm::dvec2(0, s / ODCfgParameters::uvlength - (double)k);
            lownearuvs.push_back(lowUV);

            in.z += innerH;
            highnears.push_back(in);
            highnearnormals.push_back(normal);
            glm::dvec2 highUV = glm::dvec2(1, s / ODCfgParameters::uvlength - (double)k);
            highnearuvs.push_back(highUV);

        }
        else
        {
            if (lownears.size() != 0)
            {
                glm::dvec3 normal = glm::dvec3(nx, -nz, ny);
                lownears.push_back(in);
                lownearnormals.push_back(normal);
                glm::dvec2 lowUV = glm::dvec2(0, s / ODCfgParameters::uvlength - (double)k);
                lownearuvs.push_back(lowUV);

                highnears.push_back(in);
                highnearnormals.push_back(normal);
                glm::dvec2 highUV = glm::dvec2(1, s / ODCfgParameters::uvlength - (double)k);
                highnearuvs.push_back(highUV);
            }
        }


        if (outerH > 0)
        {
            glm::dvec3 normal = glm::dvec3(nx, -nz, ny);
            lowfars.push_back(out);
            lowfarnormals.push_back(normal);
            glm::dvec2 lowUV = glm::dvec2(0, s / ODCfgParameters::uvlength - (double)k);
            lowfaruvs.push_back(lowUV);

            out.z += outerH;
            highfars.push_back(out);
            highfarnormals.push_back(normal);
            glm::dvec2 highUV = glm::dvec2(1, s / ODCfgParameters::uvlength - (double)k);
            highfaruvs.push_back(highUV);
        }

        if (i != 0)
        {
            bool add = true;
            if (lastRound)
            {
                if (nears.size() > 1)
                {
                    glm::dvec3 last = nears[nears.size() - 1];
                    if (distance(in, last) < skipdis)
                    {
                        add = false;
                    }
                }
            }

            if (add)
            {
                nears.push_back(in);
                nearNormals.push_back(inNormal);
                nearUVs.push_back(inUV);

                fars.push_back(out);
                farNormals.push_back(outNormal);
                farUVs.push_back(outUV);
            }
            else 
            {
                nears.push_back(in);
                nearNormals.push_back(inNormal);
                nearUVs.push_back(inUV);

                fars.push_back(out);
                farNormals.push_back(outNormal);
                farUVs.push_back(outUV);
            }
        }

        //CollectRoadMark(s-owner->start, i, out, outNormal, Tx, Ty, Tz);
    }

    void ODLane::GenerateRoadMark(double s, int i, ODLaneSectionPtr owner, ODRoad& road )
    {
        double disIn = owner->GetDistanceToLane(s, i);
        double disOut = disIn + owner->GetLaneWidth(s, i);
        double disOut1 = disOut + 1.0f;

        glm::dvec3 xyPoint(0, 0, 0);
        road.EvaluatePoint2D(s, xyPoint);
        double zPoint = road.EvaluateElevation(s);


        double alpha = 0;
        double beta = road.EvaluateSlope(s);
        double gamma = xyPoint.z;

        double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma));
        double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma));
        double Tz = (sin(alpha) * cos(beta));

        double nx = sin(alpha) * sin(gamma) + cos(alpha) * sin(beta) * cos(gamma);
        double ny = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
        double nz = cos(alpha) * cos(beta);

        glm::dvec3 in = glm::dvec3(xyPoint.x + Tx * disIn, -(xyPoint.y + Ty * disIn), zPoint + Tz * disIn);
        glm::dvec3 out = glm::dvec3(xyPoint.x + Tx * disOut, -(xyPoint.y + Ty * disOut), zPoint + Tz * disOut);
        glm::dvec3 out1 = glm::dvec3(xyPoint.x + Tx * disOut1, -(xyPoint.y + Ty * disOut1), zPoint + Tz * disOut1);

        glm::dvec3 dir = normalize(out - out1);
        glm::dvec3 outNormal = glm::dvec3(nx, ny, nz);

        CollectRoadMark(s - owner->start, i, out, outNormal, dir.x, dir.y, dir.z);
    }

    void ODLane::CollectRoadMark(double s, int i, const glm::dvec3& ref, const glm::dvec3& refN, double Tx, double Ty, double Tz)
    {
        ODRoadMarkCurvePtr roadmark = GetRoadmarkCurve(s);

        if (roadmark == nullptr || roadmark->type == ODRoadMarkCurve::ERM_None)
            return;

        roadmark->CollectRoadMark(s, ref, refN, Tx, Ty, Tz);
    }

    void ODLane::CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangents, vector<glm::dvec3>& binormals, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t side, vector<string>& mats)
    {
        std::vector<glm::dvec3>& evenV = (side == 0) ? fars : nears;
        std::vector<glm::dvec3>& oddV = (side == 0) ? nears : fars;

        std::vector<glm::dvec3>& evenN = (side == 0) ? farNormals : nearNormals;
        std::vector<glm::dvec3>& oddN = (side == 0) ? nearNormals : farNormals;

        std::vector<glm::dvec2>& evenUV = (side == 0) ? farUVs : nearUVs;
        std::vector<glm::dvec2>& oddUV = (side == 0) ? nearUVs : farUVs;

        for (int i = 0; i < (int)(nears.size() + fars.size() - 2); ++i)
        {
            {
                for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                {
                    int k = index % 2;
                    if (k == 0)
                    {
                        int l = index / 2;
                        vertices.push_back(evenV[l]);
                        normals.push_back(evenN[l]);
                        glm::dvec3 tangent = GetRandomTangent(evenN[l]);
                        glm::dvec3 binormal = cross(evenN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(evenUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                    else if (k != 0)
                    {
                        int l = (index - 1) / 2;
                        vertices.push_back(oddV[l]);
                        normals.push_back(oddN[l]);
                        glm::dvec3 tangent = GetRandomTangent(oddN[l]);
                        glm::dvec3 binormal = cross(oddN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(oddUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                }
            }
        }

        std::vector<glm::dvec3>& NNevenV = (side != 0) ? lownears : highnears;
        std::vector<glm::dvec3>& NNoddV = (side != 0) ? highnears : lownears;

        std::vector<glm::dvec3>& NNevenN = (side != 0) ? lownearnormals : highnearnormals;
        std::vector<glm::dvec3>& NNoddN = (side != 0) ? highnearnormals : lownearnormals;

        std::vector<glm::dvec2>& NNevenUV = (side != 0) ? lownearuvs : highnearuvs;
        std::vector<glm::dvec2>& NNoddUV = (side != 0) ? highnearuvs : lownearuvs;

        for (int i = 0; i < (int)(highnears.size() + lownears.size() - 2); ++i)
        {
            {
                for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                {
                    int k = index % 2;
                    if (k == 0)
                    {
                        int l = index / 2;
                        vertices.push_back(NNevenV[l]);
                        normals.push_back(NNevenN[l]);
                        glm::dvec3 tangent = GetRandomTangent(NNevenN[l]);
                        glm::dvec3 binormal = cross(NNevenN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(NNevenUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                    else if (k != 0)
                    {
                        int l = (index - 1) / 2;
                        vertices.push_back(NNoddV[l]);
                        normals.push_back(NNoddN[l]);
                        glm::dvec3 tangent = GetRandomTangent(NNoddN[l]);
                        glm::dvec3 binormal = cross(NNoddN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(NNoddUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                }
            }
        }

        std::vector<glm::dvec3>& FevenV = (side == 0) ? lowfars : highfars;
        std::vector<glm::dvec3>& FoddV = (side == 0) ? highfars : lowfars;

        std::vector<glm::dvec3>& FevenN = (side == 0) ? lowfarnormals : highfarnormals;
        std::vector<glm::dvec3>& FoddN = (side == 0) ? highfarnormals : lowfarnormals;

        std::vector<glm::dvec2>& FevenUV = (side == 0) ? lowfaruvs : highfaruvs;
        std::vector<glm::dvec2>& FoddUV = (side == 0) ? highfaruvs : lowfaruvs;

        for (int i = 0; i < (int)(highfars.size() + lowfars.size() - 2); ++i)
        {
            {
                for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                {
                    int k = index % 2;
                    if (k == 0)
                    {
                        int l = index / 2;
                        vertices.push_back(FevenV[l]);
                        normals.push_back(FevenN[l]);
                        glm::dvec3 tangent = GetRandomTangent(FevenN[l]);
                        glm::dvec3 binormal = cross(FevenN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(FevenUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                    else if (k != 0)
                    {
                        int l = (index - 1) / 2;
                        vertices.push_back(FoddV[l]);
                        normals.push_back(FoddN[l]);
                        glm::dvec3 tangent = GetRandomTangent(FoddN[l]);
                        glm::dvec3 binormal = cross(FoddN[l], tangent);
                        tangents.push_back(tangent);
                        binormals.push_back(binormal);
                        uvs.push_back(FoddUV[l]);
                        indices.push_back(count);
                        mats.push_back(laneType);
                        ++count;
                    }
                }
            }
        }


        // sewing height edges
        // front side
        if (lownears.size() > 0 && lowfars.size() > 0 && highnears.size() > 0 && highfars.size() > 0)
        {
            std::vector<glm::dvec3>   lowfronts;
            std::vector<glm::dvec3>   lowfrontNormals;
            std::vector<glm::dvec2>   lowfrontUVs;

            std::vector<glm::dvec3>   highfronts;
            std::vector<glm::dvec3>   highfrontNormals;
            std::vector<glm::dvec2>   highfrontUVs;

            lowfronts.push_back(lownears[0]);
            lowfrontNormals.push_back(lownearnormals[0]);
            lowfrontUVs.push_back(glm::dvec2(0, 0));

            lowfronts.push_back(lowfars[0]);
            lowfrontNormals.push_back(lowfrontNormals[0]);
            lowfrontUVs.push_back(glm::dvec2(0, 1));

            highfronts.push_back(highnears[0]);
            highfrontNormals.push_back(highnearnormals[0]);
            highfrontUVs.push_back(glm::dvec2(1, 0));

            highfronts.push_back(highfars[0]);
            highfrontNormals.push_back(highfarnormals[0]);
            highfrontUVs.push_back(glm::dvec2(1, 1));

            std::vector<glm::dvec3>& NevenV = (side == 0) ? lowfronts : highfronts;
            std::vector<glm::dvec3>& NoddV = (side == 0) ? highfronts : lowfronts;

            std::vector<glm::dvec3>& NevenN = (side == 0) ? lowfrontNormals : highfrontNormals;
            std::vector<glm::dvec3>& NoddN = (side == 0) ? highfrontNormals : lowfrontNormals;

            std::vector<glm::dvec2>& NevenUV = (side != 0) ? lowfrontUVs : highfrontUVs;
            std::vector<glm::dvec2>& NoddUV = (side != 0) ? highfrontUVs : lowfrontUVs;
            for (int i = 0; i < (int)(lowfronts.size() + highfronts.size() - 2); ++i)
            {
                {
                    for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                    {
                        int k = index % 2;
                        if (k == 0)
                        {
                            int l = index / 2;
                            vertices.push_back(NevenV[l]);
                            normals.push_back(NevenN[l]);
                            glm::dvec3 tangent = GetRandomTangent(NevenN[l]);
                            glm::dvec3 binormal = cross(NevenN[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(NevenUV[l]);
                            indices.push_back(count);
                            mats.push_back(laneType);
                            ++count;
                        }
                        else if (k != 0)
                        {
                            int l = (index - 1) / 2;
                            vertices.push_back(NoddV[l]);
                            normals.push_back(NoddN[l]);
                            glm::dvec3 tangent = GetRandomTangent(NoddN[l]);
                            glm::dvec3 binormal = cross(NoddN[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(NoddUV[l]);
                            indices.push_back(count);
                            mats.push_back(laneType);
                            ++count;
                        }
                    }
                }
            }
        }

        // back side
        if (lownears.size() > 0 && lowfars.size() > 0 && highnears.size() > 0 && highfars.size() > 0)
        {
            std::vector<glm::dvec3>   lowfronts;
            std::vector<glm::dvec3>   lowfrontNormals;
            std::vector<glm::dvec2>  lowfrontUVs;

            std::vector<glm::dvec3>   highfronts;
            std::vector<glm::dvec3>   highfrontNormals;
            std::vector<glm::dvec2>  highfrontUVs;

            lowfronts.push_back(lownears[lownears.size() - 1]);
            lowfrontNormals.push_back(lownearnormals[lownearnormals.size() - 1]);
            lowfrontUVs.push_back(glm::dvec2(0, 0));

            lowfronts.push_back(lowfars[lowfars.size() - 1]);
            lowfrontNormals.push_back(lowfrontNormals[lowfrontNormals.size() - 1]);
            lowfrontUVs.push_back(glm::dvec2(0, 1));

            highfronts.push_back(highnears[highnears.size() - 1]);
            highfrontNormals.push_back(highnearnormals[highnearnormals.size() - 1]);
            highfrontUVs.push_back(glm::dvec2(1, 0));

            highfronts.push_back(highfars[highfars.size() - 1]);
            highfrontNormals.push_back(highfarnormals[highfarnormals.size() - 1]);
            highfrontUVs.push_back(glm::dvec2(1, 1));

            std::vector<glm::dvec3>& NevenV = (side != 0) ? lowfronts : highfronts;
            std::vector<glm::dvec3>& NoddV = (side != 0) ? highfronts : lowfronts;

            std::vector<glm::dvec3>& NevenN = (side != 0) ? lowfrontNormals : highfrontNormals;
            std::vector<glm::dvec3>& NoddN = (side != 0) ? highfrontNormals : lowfrontNormals;

            std::vector<glm::dvec2>& NevenUV = (side != 0) ? lowfrontUVs : highfrontUVs;
            std::vector<glm::dvec2>& NoddUV = (side != 0) ? highfrontUVs : lowfrontUVs;
            for (int i = 0; i < (int)(lowfronts.size() + highfronts.size() - 2); ++i)
            {
                {
                    for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                    {
                        int k = index % 2;
                        if (k == 0)
                        {
                            int l = index / 2;
                            vertices.push_back(NevenV[l]);
                            normals.push_back(NevenN[l]);
                            glm::dvec3 tangent = GetRandomTangent(NevenN[l]);
                            glm::dvec3 binormal = cross(NevenN[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(NevenUV[l]);
                            indices.push_back(count);
                            mats.push_back(laneType);
                            ++count;
                        }
                        else if (k != 0)
                        {
                            int l = (index - 1) / 2;
                            vertices.push_back(NoddV[l]);
                            normals.push_back(NoddN[l]);
                            glm::dvec3 tangent = GetRandomTangent(NoddN[l]);
                            glm::dvec3 binormal = cross(NoddN[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(NoddUV[l]);
                            indices.push_back(count);
                            mats.push_back(laneType);
                            ++count;
                        }
                    }
                }
            }
        }
    }

    void ODLane::CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangents, vector<glm::dvec3>& binormals, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t side, vector<string>& mats)
    {
        for (const auto& rmIt : roadmarks)
        {
            (*rmIt).CollectRoadMarkTriangles(vertices, normals, tangents, binormals, uvs, indices, count, side, mats);
        }
    }

    void ODLane::FindClosetPointWith(const glm::dvec3& input, glm::dvec3& outpoint)
    {
        for (glm::dvec3 v : nears)
        {
            double d1 = distance(input, v);
            double d2 = distance(input, outpoint);
            if (d1 < d2)
                outpoint = v;
        }

        for (glm::dvec3 v : fars)
        {
            double d1 = distance(input, v);
            double d2 = distance(input, outpoint);
            if (d1 < d2)
                outpoint = v;
        }
    }


    /* Road Mark Curve*/
    const double ODRoadMarkCurve::RoadMarkBroken = 300.0;
    const double ODRoadMarkCurve::MinRoadMarkWidth = 12.0;

    int ODRoadMarkCurve::GetOffsetIndex(double s)
    {
        s -= start;
        if (type == ERM_None)
            return -1;
        if (type != ERM_Broken)
            return 0;

        int k = (int)s / (int)ODRoadMarkCurve::RoadMarkBroken;

        if ((k % 2) != 0)
            return -1;

        return k / 2;
    }

    void ODRoadMarkCurve::CollectRoadMark(double s, const glm::dvec3& ref, const glm::dvec3& refN, double Tx, double Ty, double Tz)
    {
        double hwidth = width;
        hwidth = fmax(hwidth, ODRoadMarkCurve::MinRoadMarkWidth);
        hwidth *= 0.5;

        glm::dvec3 in = glm::dvec3(ref.x - Tx * hwidth, ref.y - Ty * hwidth, (ref.z + 5.0) - Tz * hwidth);
        glm::dvec3 out = glm::dvec3(ref.x + Tx * hwidth, ref.y + Ty * hwidth, (ref.z + 5.0) + Tz * hwidth);

        glm::dvec2 in_uv = glm::dvec2(0.5 + hwidth / 100.0, s / 300.0);
        glm::dvec2 out_uv = glm::dvec2(0.5 - hwidth / 100.0, s / 300.0);

        int index = GetOffsetIndex(s);

        if (index == -1)
            return;

        if (index == rm_fars.size())
        {
            std::vector<glm::dvec3> far;
            rm_fars.push_back(far);
        }
        if (index < rm_fars.size())
        {
            std::vector<glm::dvec3>& f = rm_fars[index];
            f.push_back(out);
        }

        if (index == rm_farUVs.size())
        {
            std::vector<glm::dvec2> far;
            rm_farUVs.push_back(far);
        }
        if (index < rm_farUVs.size())
        {
            std::vector<glm::dvec2>& f = rm_farUVs[index];
            f.push_back(out_uv);
        }

        if (index == rm_Normals.size())
        {
            std::vector<glm::dvec3> far;
            rm_Normals.push_back(far);
        }
        if (index < rm_Normals.size())
        {
            std::vector<glm::dvec3>& f = rm_Normals[index];
            f.push_back(refN);
        }


        if (index == rm_nears.size())
        {
            std::vector<glm::dvec3> near;
            rm_nears.push_back(near);
        }
        if (index < rm_nears.size())
        {
            std::vector<glm::dvec3>& n = rm_nears[index];
            n.push_back(in);
        }
        if (index == rm_nearUVs.size())
        {
            std::vector<glm::dvec2> near;
            rm_nearUVs.push_back(near);
        }
        if (index < rm_nearUVs.size())
        {
            std::vector<glm::dvec2>& n = rm_nearUVs[index];
            n.push_back(in_uv);
        }
    }

    void ODRoadMarkCurve::CollectRoadMarkTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangents, vector<glm::dvec3>& binormals, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, std::uint32_t side, vector<string>& mats) const
    {
        for (int rm = 0; rm < rm_nears.size() && rm < rm_fars.size() && rm < rm_Normals.size(); ++rm)
        {
            std::vector<glm::dvec3> nears = rm_nears[rm];
            std::vector<glm::dvec3> fars = rm_fars[rm];

            std::vector<glm::dvec3>& evenV = (side == 0) ? fars : nears;
            std::vector<glm::dvec3>& oddV = (side == 0) ? nears : fars;

            const std::vector<glm::dvec2>& evenUV = (side == 0) ? rm_farUVs[rm] : rm_nearUVs[rm];
            const std::vector<glm::dvec2>& oddUV = (side == 0) ? rm_nearUVs[rm] : rm_farUVs[rm];

            std::vector<glm::dvec3> rm_N = rm_Normals[rm];

            for (int i = 0; i < (nears.size() + fars.size() - 2); ++i)
            {
                {
                    for (int index = (i % 2 == 0) ? i : i + 2; (i % 2 == 0) ? (index < i + 3) : (index >= i); (i % 2 == 0) ? ++index : --index)
                    {
                        int k = index % 2;
                        if (k == 0)
                        {
                            int l = index / 2;
                            vertices.push_back(evenV[l]);
                            normals.push_back(rm_N[l]);
                            glm::dvec3 tangent = GetRandomTangent(rm_N[l]);
                            glm::dvec3 binormal = cross(rm_N[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(evenUV[l]);
                            indices.push_back(count);
                            mats.push_back(string("RoadMark"));
                            ++count;
                        }
                        else if (k != 0)
                        {
                            int l = (index - 1) / 2;
                            vertices.push_back(oddV[l]);
                            normals.push_back(rm_N[l]);
                            glm::dvec3 tangent = GetRandomTangent(rm_N[l]);
                            glm::dvec3 binormal = cross(rm_N[l], tangent);
                            tangents.push_back(tangent);
                            binormals.push_back(binormal);
                            uvs.push_back(oddUV[l]);
                            indices.push_back(count);
                            mats.push_back(string("RoadMark"));
                            ++count;
                        }
                    }
                }
            }
        }
    }

    /* Signal */
    ODSignal::ODSignal(double in_s, double in_t, const string& in_id, const string& in_name, const string& in_bDynamic, const string& in_orientation, double in_zOffset, const string& in_country, const string& in_type, const string& in_subtype, double in_value, const string& in_unit, double in_height, double in_width, const string& in_text, double in_hOffset, double in_pitch, double in_roll)
    {
        s = in_s;
        t = in_t;
        id = in_id;
        name = in_name;
        if (in_bDynamic == string("yes"))
        {
            bDynamic = true;
        }
        else
        {
            bDynamic = false;
        }

        if (in_orientation == string("+"))
        {
            orientation = EST_Positive;
        }
        else if (in_orientation == string("-"))
        {
            orientation = EST_Negative;
        }
        else
        {
            orientation = EST_Both;
        }

        zOffset = in_zOffset;
        country = in_country;
        type = in_type;
        subtype = in_subtype;
        value = in_value;
        unit = in_unit;
        height = in_height;
        width = in_width;
        text = in_text;
        hOffset = in_hOffset;
        pitch = in_pitch;
        roll = in_roll;
    }

    ODSignalReference::ODSignalReference(double in_s, double in_t, const string& in_id, const string& in_orientation)
    {
        s = in_s;
        t = in_t;
        id = in_id;
        if (in_orientation == string("+"))
        {
            orientation = ODSignal::EST_Positive;
        }
        else if (in_orientation == string("-"))
        {
            orientation = ODSignal::EST_Negative;
        }
        else
        {
            orientation = ODSignal::EST_Both;
        }
    }


    /* Connection */
    ODConnection::ODConnection(const string& in_id,const string& inR, const string& outR, const string& connType)
        : id(in_id), inRoad(inR), outRoad(outR)
    {
        dir = 1;
        if (connType == string("end"))
        {
            dir = -1;
        }
    }

    void ODConnection::AddLaneLink(int32_t from, int32_t to)
    {
        lanelinkmap[from] = to;
    }

    int32_t ODConnection::FindRoadOuterLaneId(int32_t side)
    {
        int32_t ret = 0;
        if (side > 0) // left
        {            
            for (auto& lane : lanelinkmap)
            {
                if (lane.first > ret)
                    ret = lane.first;
            }
            return ret;
        }

        else
        {
            for (auto& lane : lanelinkmap)
            {
                if (lane.first < ret)
                    ret = lane.first;
            }
            return ret;
        }        
    }

    int32_t ODConnection::FindLinkOuterLandId(int32_t side)
    {
        int32_t ret = 0;
        if (side > 0) // left
        { 
            for (auto& lane : lanelinkmap)
            {
                if (lane.second > ret)
                    ret = lane.second;
            }
            return ret;
        }

        else
        {
            for (auto& lane : lanelinkmap)
            {
                if (lane.second < ret)
                    ret = lane.second;
            }
            return ret;
        }
    }


    /* Junction */
    ODJunction::ODJunction(const string& id, const string& name)
        : ODElement(id, name)
    {}

    void ODJunction::AddConnection(ODConnectionPtr conn)
    {
        ConnectionMap[conn->id] = conn;
    }

    void ODJunction::AddController(const string& in_id, const string& type)
    {
        ControllerMap[in_id] = type;
    }

    // TrangulatePoly: copy from UE4 GeomTools::TriangulatePoly
    struct SMTriangle
    {
        glm::dvec3   Vertices[3];
    };


    /** Determines whether two edges may be merged. */
    static bool AreEdgesMergeable(
        const glm::dvec3& V0,
        const glm::dvec3& V1,
        const glm::dvec3& V2
    )
    {
        const glm::dvec3 MergedEdgeVector = V2 - V0;
        const double MergedEdgeLengthSquared = glm::length(MergedEdgeVector) * glm::length(MergedEdgeVector);
        if (MergedEdgeLengthSquared > 0.00001)
        {
            // Find the vertex closest to A1/B0 that is on the hypothetical merged edge formed by A0-B1.
            const double IntermediateVertexEdgeFraction =
                glm::dot((V2 - V0), (V1 - V0)) / MergedEdgeLengthSquared;
            
            const glm::dvec3 InterpolatedVertex = V0 + (V2 - V0) * IntermediateVertexEdgeFraction;

            // The edges are merge-able if the interpolated vertex is close enough to the intermediate vertex.
            auto rest = glm::equal(InterpolatedVertex, V1);
            return rest.x && rest.y && rest.z;
        }
        else
        {
            return true;
        }
    }

    /** Given three direction vectors, indicates if A and B are on the same 'side' of Vec. */
    static bool  VectorsOnSameSide(const glm::dvec3& Vec, const glm::dvec3& A, const glm::dvec3& B)
    {
        const glm::dvec3 CrossA = glm::cross( Vec , A) ;
        const glm::dvec3 CrossB = glm::cross( Vec , B );
        double dotvalue = glm::dot(CrossA, CrossB);

        return !((*(std::uint64_t*)&dotvalue) >= (std::uint64_t)0x8000000000000000);
    }

    /** Util to see if P lies within triangle created by A, B and C. */
    static bool PointInTriangle(const glm::dvec3& A, const glm::dvec3& B, const glm::dvec3& C, const glm::dvec3& P)
    {
        // Cross product indicates which 'side' of the vector the point is on
        // If its on the same side as the remaining vert for all edges, then its inside.	
        if (VectorsOnSameSide(B - A, P - A, C - A) &&
            VectorsOnSameSide(C - B, P - B, A - B) &&
            VectorsOnSameSide(A - C, P - C, B - C))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    static bool glmTrangulatePoly(vector<SMTriangle>&OutTris, const vector<glm::dvec3>&InPoly, const glm::dvec3& FaceNormal, bool bKeepColinearVertices)
    {
        // Can't work if not enough verts for 1 triangle
        if (InPoly.size()< 3)
        {
            // Return true because poly is already a tri
            return false;
        }

        // Vertices of polygon in order - make a copy we are going to modify.
        vector<glm::dvec3> PolyVerts = InPoly;

        // Keep iterating while there are still vertices
        while (true)
        {
            if (!bKeepColinearVertices)
            {
                // Cull redundant vertex edges from the polygon.
                for (int VertexIndex = 0; VertexIndex < PolyVerts.size(); VertexIndex++)
                {
                    const int I0 = (VertexIndex + 0) % PolyVerts.size();
                    const int I1 = (VertexIndex + 1) % PolyVerts.size();
                    const int I2 = (VertexIndex + 2) % PolyVerts.size();
                    if (AreEdgesMergeable(PolyVerts[I0], PolyVerts[I1], PolyVerts[I2]))
                    {
                        PolyVerts.erase(PolyVerts.begin() + I1);
                        VertexIndex--;
                    }
                }
            }

            if (PolyVerts.size() < 3)
            {
                break;
            }
            else
            {
                // Look for an 'ear' triangle
                bool bFoundEar = false;
                for (int EarVertexIndex = 0; EarVertexIndex < PolyVerts.size(); EarVertexIndex++)
                {
                    // Triangle is 'this' vert plus the one before and after it
                    const int AIndex = (EarVertexIndex == 0) ? PolyVerts.size() - 1 : EarVertexIndex - 1;
                    const int BIndex = EarVertexIndex;
                    const int CIndex = (EarVertexIndex + 1) % PolyVerts.size();

                    // Check that this vertex is convex (cross product must be positive)
                    const glm::dvec3 ABEdge = PolyVerts[BIndex] - PolyVerts[AIndex];
                    const glm::dvec3 ACEdge = PolyVerts[CIndex] - PolyVerts[AIndex];
                    const double TriangleDeterminant = glm::dot(glm::cross(ABEdge, ACEdge), FaceNormal);
                    if ((*(std::uint64_t*)&TriangleDeterminant) >= (std::uint64_t)0x8000000000000000)
                    {
                        continue;
                    }

                    bool bFoundVertInside = false;
                    // Look through all verts before this in array to see if any are inside triangle
                    for (int VertexIndex = 0; VertexIndex < PolyVerts.size(); VertexIndex++)
                    {
                        if (VertexIndex != AIndex && VertexIndex != BIndex && VertexIndex != CIndex &&
                            PointInTriangle(PolyVerts[AIndex], PolyVerts[BIndex], PolyVerts[CIndex], PolyVerts[VertexIndex]))
                        {
                            bFoundVertInside = true;
                            break;
                        }
                    }

                    // Triangle with no verts inside - its an 'ear'! 
                    if (!bFoundVertInside)
                    {
                        // Add to output list..
                        SMTriangle NewTri;
                        NewTri.Vertices[0] = PolyVerts[AIndex];
                        NewTri.Vertices[1] = PolyVerts[BIndex];
                        NewTri.Vertices[2] = PolyVerts[CIndex];
                        OutTris.push_back(NewTri);

                        // And remove vertex from polygon
                        PolyVerts.erase(PolyVerts.begin()+EarVertexIndex);

                        bFoundEar = true;
                        break;
                    }
                }

                // If we couldn't find an 'ear' it indicates something is bad with this polygon - discard triangles and return.
                if (!bFoundEar)
                {                   
                    OutTris.empty();
                    return false;
                }
            }
        }

        return true;
    }


    void ODJunction::CollectTriangles(vector<glm::dvec3>& vertices, vector<glm::dvec3>& normals, vector<glm::dvec3>& tangents, vector<glm::dvec3>& binormals, vector<glm::dvec2>& uvs, vector<std::uint32_t>& indices, std::uint32_t& count, vector<string>& mats)
    {
        if (verticesCCW.size() < 3)
            return;

        vector<glm::dvec3> InPoly;
        InPoly.reserve(verticesCCW.size());
        for (int index = verticesCCW.size()-1; index >=0; --index)
        {
            InPoly.push_back(verticesCCW[index]);
        }

        vector<SMTriangle> OutTris;
        glm::dvec3 normal(0, 0, 1);
        bool bTriangulated = glmTrangulatePoly(OutTris, InPoly, normal, false);
        if (!bTriangulated)
        {
            bTriangulated = glmTrangulatePoly(OutTris, verticesCCW, normal, false);
        }

        if (bTriangulated)
        {
            for (auto& smTri : OutTris)
            {
                for (int32_t i = 2; i >= 0; --i)
                {
                    vertices.push_back(smTri.Vertices[i]);

                    normals.push_back(normal);
                    glm::dvec3 tangent = GetRandomTangent(normal);
                    tangents.push_back(tangent);
                    binormals.push_back(glm::cross(normal, tangent));
                    uvs.push_back(glm::dvec2(smTri.Vertices[i].x / 100.0, smTri.Vertices[i].y / 100.0));
                    indices.push_back(count);
                    mats.push_back(string("junction"));
                    ++count;
                }
            }
        }
    }


    void ODJunction::SortRoadsCCW(std::map<string, std::weak_ptr<ODRoad>> incomingRoads, std::vector<std::pair<string, float>>& thetas, glm::dvec3& center)
    {
        std::map<string, glm::dvec3>  roadHeadsPoints;

        for (const auto& rIt : incomingRoads)
        {
            ODRoadRef road = rIt.second;
            if (auto tmp = road.lock())
            {
                glm::dvec3 head;
                tmp->GetHeadCenterPointbyRef(head, *this, ODLink::ELT_JUNCTION);
                roadHeadsPoints[rIt.first] = head;
                center += head;
            }
        }

        center *= (1.0f / incomingRoads.size());                                                

        for (const auto& hIt : roadHeadsPoints)
        {
            glm::dvec3 headP = hIt.second;
            glm::dvec3 dir = headP - center;            

            double val = 0;
            if (dir.x != 0)
            {
                val = atan2(dir.y, dir.x);
            }
            else
            {
                val = PI * 0.5f * dir.y / abs(dir.y);
            }

            thetas.push_back(std::make_pair(hIt.first, (float)val));
        }

        auto cmp = [](const std::pair<string, float>& x, const std::pair<string, float>& y)
        {
            return x.second > y.second;
        };

        std::sort(thetas.begin(), thetas.end(), cmp);
    }

    string ODJunction::FindConnectRoad(const string& in, const string& out, ODSystem& odroot)
    {
        for (const auto&cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;

            if (conn->inRoad == in)
            {
                const string& linkedid = conn->outRoad;
                ODRoadPtr linkedroad = odroot.GetRoad(linkedid);
                if (linkedroad != nullptr)
                {
                    if (linkedroad->IsConnected(out, ODLink::ELT_ROAD) != 0)
                        return linkedroad->id;
                }
            }
        }

        return string("");
    }

    int32_t ODJunction::GetInComingOuterSideCCW(const string& incoming, ODSystem& odroot) const
    {
        for (const auto& connection : ConnectionMap)
        {
            const ODConnectionPtr& cptr = connection.second;
            if (cptr->inRoad == incoming)
            {
                ODRoadPtr connRoad = odroot.GetRoad(cptr->outRoad);
                ODLinkPtr linkptr = (cptr->dir == 1) ? connRoad->predecessor : connRoad->successor;
                if (linkptr != nullptr)
                {
                    if (linkptr->link == incoming )
                    {
                        return linkptr->dir * cptr->dir;                    
                    }
                }
            }
        }

        return 0;
    }


    int32_t ODJunction::FindRoadOuterLaneId(const string& incoming, const string& connRoad, int32_t side)
    {
        int32_t ret = 0;
        for (auto& cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;
            if (conn->inRoad == incoming
                && conn->outRoad == connRoad)
            {
                return conn->FindRoadOuterLaneId(side);
            }
        }
        return ret;
    }

    int32_t ODJunction::FindLinkOuterLandId(const string& incoming, const string& connRoad, int32_t side)
    {
        int32_t ret = 0;
        for (auto& cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;
            if (conn->inRoad == incoming
                && conn->outRoad == connRoad)
            {
                return conn->FindLinkOuterLandId(side);
            }
        }
        return ret;
    }

    ODConnectionPtr ODJunction::GetConnection(const string& incoming, const string& conn)
    {
        for (const auto& connection : ConnectionMap)
        {
            ODConnectionPtr cptr = connection.second;
            if (cptr->inRoad == incoming
                && cptr->outRoad == conn)
            {
                return cptr;
            }
        }
        return nullptr;
    }


    bool ODJunction::IsLinkedByOtherConnection(const string& incoming, const string& conn, int32_t laneid)
    {
        for (const auto& connection : ConnectionMap)
        {
            const ODConnectionPtr& cptr = connection.second;
            if (cptr->inRoad == incoming
                && cptr->outRoad != conn)
            {
                for (auto& link : cptr->lanelinkmap)
                {
                    if (link.first == laneid)
                        return true;
                }
            }
        }
        return false;
    }

    int32_t ODJunction::FindConnLaneId(const string& incoming, const string& conn, int32_t laneid)
    {
        int32_t ret = 0;
        for (const auto& connection : ConnectionMap)
        {
            const ODConnectionPtr& cptr = connection.second;
            if (cptr->inRoad  == incoming
                && cptr->outRoad  == conn)
            {
                for (auto& link : cptr->lanelinkmap)
                {
                    if (link.first == laneid)
                        return link.second;
                }
            }
        }
        return ret;
    }

    void ODJunction::GenerateVertices(ODSystem& odroot)
    {
        map<string, ODRoadRef>      incomingRoads;
        map<string, ODRoadRef>      junctionRoads;
        map<string, ODRoadRef>      outRoads;

        CollectInComingRoad(incomingRoads, odroot);
        CollectJunctionRoad(junctionRoads, odroot);
        CollectOutRoad(outRoads, incomingRoads, odroot);

        vector<std::pair<string, float> > thetas;
        thetas.reserve(incomingRoads.size() + outRoads.size());

        //merge incoming roads and out roads

        for (auto& rIt : outRoads)
        {            
            incomingRoads[rIt.first] = rIt.second;
        }

        glm::dvec3 center(0, 0, 0);
        SortRoadsCCW(incomingRoads, thetas, center);

        // building road links
        std::vector<std::pair<string, float>>::iterator iter;
        std::vector<std::pair<string, float>>::iterator nextiter;

        verticesCCW.clear();
        std::uint32_t vercount = 0;
        for (iter = thetas.begin(); iter != thetas.end(); ++iter)
        {
            nextiter = iter;
            ++nextiter;
            if (nextiter == thetas.end())
            {
                nextiter = thetas.begin();
            }

            string ccroad1 = iter->first;
            string ccroad2 = nextiter->first;

            string conn = FindConnectRoad(ccroad1, ccroad2,odroot);

            string startRoad = ccroad1;
            string endRoad = ccroad2;
            int dir = 1;
            if (!conn.empty())
            {
                // find other connection way
                string revconn = FindConnectRoad(ccroad2, ccroad1, odroot);
                if (!revconn.empty())
                {
                    int32_t cc1outerside = GetInComingOuterSideCCW(ccroad1, odroot);
                    int32_t maxouterLandid = FindRoadOuterLaneId(ccroad1, conn, cc1outerside);

                    ODRoadPtr revconnptr = odroot.GetRoad(revconn);
                    int32_t maxLinkedLeftLaneid = FindLinkOuterLandId(ccroad2, revconn, 1);
                    //int32_t maxLinkedRightLaneid = FindLinkOuterLandId(ccroad2, revconn, -1);

                    int32_t maxLeft = revconnptr->FindLaneLinkedIdWith(ccroad1, maxLinkedLeftLaneid);
                    int32_t maxRight = revconnptr->FindLaneLinkedIdWith(ccroad1, maxLinkedLeftLaneid);

                    if ((maxLeft * maxouterLandid > 0 && fabs(maxLeft) > fabs(maxouterLandid)) ||
                        (maxRight * maxouterLandid > 0 && fabs(maxRight) > fabs(maxouterLandid))
                        )
                    {
                        conn = string("");
                    }
                }
            }
            if (conn.empty())
            {
                conn = FindConnectRoad(ccroad2, ccroad1, odroot);
                startRoad = ccroad2;
                endRoad = ccroad1;
                dir = -1;
            }

            ODConnectionPtr cptr = GetConnection(startRoad, conn);

            if (!conn.empty())
            {
                std::map<string, ODRoadRef >::iterator iterconn = junctionRoads.find(conn);
                if (iterconn != junctionRoads.end())
                {
                    if (auto tmp = iterconn->second.lock())
                    {
                        int32_t rightlane = FindRoadOuterLaneId(startRoad, conn, -1);
                        int32_t leftlane = FindRoadOuterLaneId(startRoad, conn, 1);

                        ODRoadPtr startRoadPtr = odroot.GetRoad(startRoad);

                        int32_t inLane = rightlane;

                        if ((rightlane == 0 || IsLinkedByOtherConnection(startRoad, conn, rightlane)) && leftlane != 0)
                        {
                            if (!IsLinkedByOtherConnection(startRoad, conn, leftlane))
                            {

                                inLane = leftlane;
                            }
                            else
                            {
                                // there is something errors, middle connection road do not used borader linked road
                                inLane = (rightlane == 0) ? leftlane : rightlane;
                            }
                        }
                        int32_t connLane = FindConnLaneId(startRoad, conn, inLane);
                        int32_t side = connLane > 0 ? 1 : -1;

                        dir = dir * cptr->dir;

                        //int lastCount = verticesCCW.size();
                        vector<glm::dvec3> connSide;
                        tmp->CollectBoundary(side, connSide, vercount);
                        if (connSide.size() > 0)
                        {

                            glm::dvec3 headp = tmp->GetEdgePoint(side, 1);
                            glm::dvec3 endp = tmp->GetEdgePoint(side, -1);



                            ODRoadPtr endRoadPtr = odroot.GetRoad(endRoad);
                            glm::dvec3 nearestHead(0, 0, 0);
                            glm::dvec3 nearestEnd(0, 0, 0);
                            // TODO: some bugs here, fix them later...
                            startRoadPtr->FindClosetPointWith(headp, nearestHead);
                            endRoadPtr->FindClosetPointWith(headp, nearestHead);
                            startRoadPtr->FindClosetPointWith(endp, nearestEnd);
                            endRoadPtr->FindClosetPointWith(endp, nearestEnd);

                            connSide[connSide.size() - 1] = nearestEnd;
                            connSide[0] = nearestHead;
                            if (dir == -1)
                            {
                                for (int index = connSide.size() - 1; index >= 0; --index)
                                {
                                    verticesCCW.push_back(connSide[index]);
                                }
                            }
                            else
                            {
                                for (int index = 0; index < connSide.size(); ++index)
                                {
                                    verticesCCW.push_back(connSide[index]);
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                ODRoadPtr ccroad1ptr = odroot.GetRoad(ccroad1);
                ODRoadPtr ccroad2ptr = odroot.GetRoad(ccroad2);
                glm::dvec3 right;
                ccroad1ptr->GetLinkEdgePointByRef(1, right, *this);
                glm::dvec3 left;
                ccroad2ptr->GetLinkEdgePointByRef(1, left, *this);
                verticesCCW.push_back(right);
                verticesCCW.push_back(left);
                vercount += 2;
            }
        }


    }

    void ODJunction::CollectJunctionRoad(map<string, ODRoadRef >&    junctionRoads, ODSystem& odroot)
    {
        for (const auto&cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;
            ODRoadPtr road = odroot.GetRoad(conn->outRoad);
            if (road != nullptr)
            {
                map<string, ODRoadRef >::iterator iter = junctionRoads.find(road->id);
                if (iter == junctionRoads.end())
                {
                    junctionRoads[road->id] = road;
                }
            }
        }
    }
    void ODJunction::CollectInComingRoad(map<string, ODRoadRef >&    incomingRoads, ODSystem& odroot)
    {
        for (const auto&cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;
            ODRoadPtr road = odroot.GetRoad(conn->inRoad);
            if (road != nullptr)
            {
                map<string, ODRoadRef >::iterator iter = incomingRoads.find(road->id);
                if (iter == incomingRoads.end())
                {
                    incomingRoads[road->id] = road;
                }
            }
        }
    }
    void ODJunction::CollectOutRoad(map<string, ODRoadRef >&    outRoads, const map<string, ODRoadRef >&    incomingRoads, ODSystem& odroot)
    {
        for (const auto&cIt : ConnectionMap)
        {
            ODConnectionPtr conn = cIt.second;
            ODRoadPtr road = odroot.GetRoad(conn->outRoad);
            if (road != nullptr)
            {
                if (road->predecessor != nullptr)
                {
                    string ele = road->predecessor->link;

                    const std::map<string, ODRoadRef >::const_iterator iter = incomingRoads.find(ele);
                    if (iter == incomingRoads.end())
                    {
                        ODRoadPtr outRoad = odroot.GetRoad(ele);
                        outRoads[outRoad->id] = outRoad;
                    }
                }
                if (road->successor != nullptr)
                {
                    string ele = road->successor->link;

                    const std::map<string, ODRoadRef >::const_iterator iter = incomingRoads.find(ele);
                    if (iter == incomingRoads.end())
                    {
                        ODRoadPtr outRoad = odroot.GetRoad(ele);
                        outRoads[outRoad->id] = outRoad;
                    }
                }
            }
        }
    }

}